#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>
#include <opencv2/opencv.hpp>
#include <ros/ros.h>

#include <iostream>
#include <vector>

using namespace std;
using namespace cv;


cv::Mat thinImage(const cv::Mat & src, const int maxIterations = -1);
void filterOver(cv::Mat thinSrc);
int Otsu(IplImage* src);

int main(int argc, char **argv) {
  cv::Mat image = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
  // cv::Mat image = cv::imread("/home/bianxu/tools/test.jpg",
  // CV_LOAD_IMAGE_COLOR);
  if (image.empty()) {
    printf("open error\n");
  }
  imshow("image", image);

  Mat image1;
  //medianBlur( image,image1, 5);  //中值滤波
  bilateralFilter(image, image1, 5, 20, 20, BORDER_REPLICATE); //双边滤波
  Mat gray;
  cvtColor(image1, gray, CV_RGB2GRAY);
  imshow("gray", gray);
  //Mat binaryimage;
  //binaryimage = gray.clone();
  //Mat binaryimage1;
  //binaryimage1 = gray.clone();
  Mat binaryimage2;
  binaryimage2 = gray.clone();
  //进行二值化处理，选择30，255.0为阈值
  //threshold(gray, binaryimage, 35, 255.0, CV_THRESH_BINARY);  //全局二值化
  //adaptiveThreshold(gray, binaryimage1, 255.0, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, 3, 5 ); //自适应二值化
  //imshow("image1", binaryimage);
  //imshow("image2", binaryimage1);
  IplImage gray1 = IplImage(gray);
  int thresh = Otsu(&gray1); //调用otsu大津法求出最佳阈值
  threshold(gray, binaryimage2, thresh, 255, CV_THRESH_BINARY); //用otsu的阈值二值化
  imshow("binaryimage", binaryimage2);

  Mat element = getStructuringElement(MORPH_RECT, Size(5, 5)); //获取自定义核
  Mat out1;
  erode(binaryimage2, out1, element);
  Mat out2;
  Canny(out1, out2, 50, 200, 3);
  imshow("Canny", out2);

  // //将原图像转换为0和1的二值图像  
	// cv::threshold(out1, out1, 128, 1, cv::THRESH_BINARY);
	// //图像细化，骨骼化  
	// cv::Mat dst = thinImage(out1, -1);
	// //过滤细化后的图像
	// filterOver(dst);
	// //二值图转化成灰度图
	// dst = dst * 255;
	// //显示图像  
	// cv::imshow("dst1", dst);




  cv::waitKey(0);
  return (0);
}


// OTSU算法求二值化阈值
int Otsu(IplImage* src)  
{  
    int height=src->height;  
    int width=src->width;      

    //histogram  
    float histogram[256] = {0};  
    for(int i=0; i < height; i++)
    {  
        unsigned char* p=(unsigned char*)src->imageData + src->widthStep * i;  
        for(int j = 0; j < width; j++) 
        {  
            histogram[*p++]++;  
        }  
    }  

    //normalize histogram & average pixel value 
    int size = height * width;  
    float u =0;
    for(int i = 0; i < 256; i++)
    {  
        histogram[i] = histogram[i] / size;  
        u += i * histogram[i];  //整幅图像的平均灰度
    }  

    int threshold;    
    float maxVariance=0;  
    float w0 = 0, avgValue  = 0;
    for(int i = 0; i < 256; i++) 
    {  
        w0 += histogram[i];  //假设当前灰度i为阈值, 0~i 灰度像素所占整幅图像的比例即前景比例
        avgValue  += i * histogram[i]; //avgValue/w0 = u0

        float t = avgValue/w0 - u;  //t=u0-u
        float variance = t * t * w0 /(1 - w0);  
        if(variance > maxVariance) 
        {  
            maxVariance = variance;  
            threshold = i;  
        }  
    }  

    return threshold;  
}

 
/**
* @brief 对输入图像进行细化,骨骼化
* @param src为输入图像,用cvThreshold函数处理过的8位灰度图像格式，元素中只有0与1,1代表有元素，0代表为空白
* @param maxIterations限制迭代次数，如果不进行限制，默认为-1，代表不限制迭代次数，直到获得最终结果
* @return 为对src细化后的输出图像,格式与src格式相同，元素中只有0与1,1代表有元素，0代表为空白
*/
cv::Mat thinImage(const cv::Mat & src, const int maxIterations)
{
	assert(src.type() == CV_8UC1);
	cv::Mat dst;
	int width = src.cols;
	int height = src.rows;
	src.copyTo(dst);
	int count = 0;  //记录迭代次数  
	while (true)
	{
		count++;
		if (maxIterations != -1 && count > maxIterations) //限制次数并且迭代次数到达  
			break;
		std::vector<uchar *> mFlag; //用于标记需要删除的点  
		//对点标记  
		for (int i = 0; i < height; ++i)
		{
			uchar * p = dst.ptr<uchar>(i);
			for (int j = 0; j < width; ++j)
			{
				//如果满足四个条件，进行标记  
				//  p9 p2 p3  
				//  p8 p1 p4  
				//  p7 p6 p5  
				uchar p1 = p[j];
				if (p1 != 1) continue;
				uchar p4 = (j == width - 1) ? 0 : *(p + j + 1);
				uchar p8 = (j == 0) ? 0 : *(p + j - 1);
				uchar p2 = (i == 0) ? 0 : *(p - dst.step + j);
				uchar p3 = (i == 0 || j == width - 1) ? 0 : *(p - dst.step + j + 1);
				uchar p9 = (i == 0 || j == 0) ? 0 : *(p - dst.step + j - 1);
				uchar p6 = (i == height - 1) ? 0 : *(p + dst.step + j);
				uchar p5 = (i == height - 1 || j == width - 1) ? 0 : *(p + dst.step + j + 1);
				uchar p7 = (i == height - 1 || j == 0) ? 0 : *(p + dst.step + j - 1);
				if ((p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9) >= 2 && (p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9) <= 6)
				{
					int ap = 0;
					if (p2 == 0 && p3 == 1) ++ap;
					if (p3 == 0 && p4 == 1) ++ap;
					if (p4 == 0 && p5 == 1) ++ap;
					if (p5 == 0 && p6 == 1) ++ap;
					if (p6 == 0 && p7 == 1) ++ap;
					if (p7 == 0 && p8 == 1) ++ap;
					if (p8 == 0 && p9 == 1) ++ap;
					if (p9 == 0 && p2 == 1) ++ap;
 
					if (ap == 1 && p2 * p4 * p6 == 0 && p4 * p6 * p8 == 0)
					{
						//标记  
						mFlag.push_back(p + j);
					}
				}
			}
		}
 
		//将标记的点删除  
		for (std::vector<uchar *>::iterator i = mFlag.begin(); i != mFlag.end(); ++i)
		{
			**i = 0;
		}
 
		//直到没有点满足，算法结束  
		if (mFlag.empty())
		{
			break;
		}
		else
		{
			mFlag.clear();//将mFlag清空  
		}
 
		//对点标记  
		for (int i = 0; i < height; ++i)
		{
			uchar * p = dst.ptr<uchar>(i);
			for (int j = 0; j < width; ++j)
			{
				//如果满足四个条件，进行标记  
				//  p9 p2 p3  
				//  p8 p1 p4  
				//  p7 p6 p5  
				uchar p1 = p[j];
				if (p1 != 1) continue;
				uchar p4 = (j == width - 1) ? 0 : *(p + j + 1);
				uchar p8 = (j == 0) ? 0 : *(p + j - 1);
				uchar p2 = (i == 0) ? 0 : *(p - dst.step + j);
				uchar p3 = (i == 0 || j == width - 1) ? 0 : *(p - dst.step + j + 1);
				uchar p9 = (i == 0 || j == 0) ? 0 : *(p - dst.step + j - 1);
				uchar p6 = (i == height - 1) ? 0 : *(p + dst.step + j);
				uchar p5 = (i == height - 1 || j == width - 1) ? 0 : *(p + dst.step + j + 1);
				uchar p7 = (i == height - 1 || j == 0) ? 0 : *(p + dst.step + j - 1);
 
				if ((p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9) >= 2 && (p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9) <= 6)
				{
					int ap = 0;
					if (p2 == 0 && p3 == 1) ++ap;
					if (p3 == 0 && p4 == 1) ++ap;
					if (p4 == 0 && p5 == 1) ++ap;
					if (p5 == 0 && p6 == 1) ++ap;
					if (p6 == 0 && p7 == 1) ++ap;
					if (p7 == 0 && p8 == 1) ++ap;
					if (p8 == 0 && p9 == 1) ++ap;
					if (p9 == 0 && p2 == 1) ++ap;
 
					if (ap == 1 && p2 * p4 * p8 == 0 && p2 * p6 * p8 == 0)
					{
						//标记  
						mFlag.push_back(p + j);
					}
				}
			}
		}
 
		//将标记的点删除  
		for (std::vector<uchar *>::iterator i = mFlag.begin(); i != mFlag.end(); ++i)
		{
			**i = 0;
		}
 
		//直到没有点满足，算法结束  
		if (mFlag.empty())
		{
			break;
		}
		else
		{
			mFlag.clear();//将mFlag清空  
		}
	}
	return dst;
}
 
/**
* @brief 对骨骼化图数据进行过滤，实现两个点之间至少隔一个空白像素
* @param thinSrc为输入的骨骼化图像,8位灰度图像格式，元素中只有0与1,1代表有元素，0代表为空白
*/
void filterOver(cv::Mat thinSrc)
{
	assert(thinSrc.type() == CV_8UC1);
	int width = thinSrc.cols;
	int height = thinSrc.rows;
	for (int i = 0; i < height; ++i)
	{
		uchar * p = thinSrc.ptr<uchar>(i);
		for (int j = 0; j < width; ++j)
		{
			// 实现两个点之间至少隔一个像素
			//  p9 p2 p3  
			//  p8 p1 p4  
			//  p7 p6 p5  
			uchar p1 = p[j];
			if (p1 != 1) continue;
			uchar p4 = (j == width - 1) ? 0 : *(p + j + 1);
			uchar p8 = (j == 0) ? 0 : *(p + j - 1);
			uchar p2 = (i == 0) ? 0 : *(p - thinSrc.step + j);
			uchar p3 = (i == 0 || j == width - 1) ? 0 : *(p - thinSrc.step + j + 1);
			uchar p9 = (i == 0 || j == 0) ? 0 : *(p - thinSrc.step + j - 1);
			uchar p6 = (i == height - 1) ? 0 : *(p + thinSrc.step + j);
			uchar p5 = (i == height - 1 || j == width - 1) ? 0 : *(p + thinSrc.step + j + 1);
			uchar p7 = (i == height - 1 || j == 0) ? 0 : *(p + thinSrc.step + j - 1);
			if (p2 + p3 + p8 + p9 >= 1)
			{
				p[j] = 0;
			}
		}
	}
}
 
